//
// Created by daybeha on 2022/7/13.
//

#include <iostream>
#include "opencv2/opencv.hpp"

#include "ros/ros.h"
#include "superglue/SuperScore.h"
#include "cv_bridge/cv_bridge.h"

using namespace std;


// 图像拼接
cv::Mat ImageSplicing(vector<cv::Mat> images,int type)
{
    if (type != 0 && type != 1)
        type = 0;

    int num = images.size();
    int newrow = 0;
    int newcol = 0;
    cv::Mat result;

    // 横向拼接
    if (type == 0)
    {
        int minrow = 10000;
        for (int i = 0; i < num; ++i)
        {
            if (minrow > images[i].rows)
                minrow = images[i].rows;
        }
        newrow = minrow;
        for (int i = 0; i < num; ++i)
        {
            int tcol = images[i].cols*minrow / images[i].rows;
            int trow = newrow;
            cv::resize(images[i], images[i], cv::Size(tcol, trow));
            newcol += images[i].cols;
            if (images[i].type() != images[0].type())
                images[i].convertTo(images[i], images[0].type());
        }
        result = cv::Mat(newrow, newcol, images[0].type(), cv::Scalar(255, 255, 255));

        cv::Range rangerow, rangecol;
        int start = 0;
        for (int i = 0; i < num; ++i)
        {
            rangerow = cv::Range((newrow - images[i].rows) / 2, (newrow - images[i].rows) / 2 + images[i].rows);
            rangecol = cv::Range(start, start + images[i].cols);
            images[i].copyTo(result(rangerow, rangecol));
            start += images[i].cols;
        }
    }
        // 纵向拼接
    else if (type == 1) {
        int mincol = 10000;
        for (int i = 0; i < num; ++i)
        {
            if (mincol > images[i].cols)
                mincol = images[i].cols;
        }
        newcol = mincol;
        for (int i = 0; i < num; ++i)
        {
            int trow = images[i].rows*mincol / images[i].cols;
            int tcol = newcol;
            cv::resize(images[i], images[i], cv::Size(tcol, trow));
            newrow += images[i].rows;
            if (images[i].type() != images[0].type())
                images[i].convertTo(images[i], images[0].type());
        }
        result = cv::Mat(newrow, newcol, images[0].type(), cv::Scalar(255, 255, 255));

        cv::Range rangerow, rangecol;
        int start = 0;
        for (int i = 0; i < num; ++i)
        {
            rangecol= cv::Range((newcol - images[i].cols) / 2, (newcol - images[i].cols) / 2 + images[i].cols);
            rangerow = cv::Range(start, start + images[i].rows);
            images[i].copyTo(result(rangerow, rangecol));
            start += images[i].rows;
        }
    }

    return result;
}

int main(int argc, char ** argv) {
    setlocale(LC_ALL, "");

    // 2.初始化 ROS 节点
    ros::init(argc, argv, "client");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;

    ros::ServiceClient client = nh.serviceClient<superglue::SuperScore>("/super_score");

    superglue::SuperScore  superScore;


    int count = 0;
    ros::Rate r(0.02);
//    ros::Duration(3.0).sleep();
//    r.sleep();      // 先等一会儿再发布，给server响应时间
    while (ros::ok()){
        stringstream ss0, ss1;
        ss0 << setfill('0') << setw(6) << count;
        // 读取图片
        cv::Mat img0 = cv::imread("/home/daybeha/Documents/Dataset/Kitti/sequences/00/image_0/"+  ss0.str() +".png",
                                  cv::IMREAD_GRAYSCALE);
        ss1 << setfill('0') << setw(6) << count+25;
        // 读取图片
        cv::Mat img1 = cv::imread("/home/daybeha/Documents/Dataset/Kitti/sequences/00/image_0/"+  ss1.str() +".png",
                              cv::IMREAD_GRAYSCALE);
//        sensor_msgs::ImagePtr pimage0 = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img0).toImageMsg();
//        sensor_msgs::ImagePtr pimage1 = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img1).toImageMsg();
        sensor_msgs::ImagePtr pimage0 = cv_bridge::CvImage(std_msgs::Header(), "mono8", img0).toImageMsg();
        sensor_msgs::ImagePtr pimage1 = cv_bridge::CvImage(std_msgs::Header(), "mono8", img1).toImageMsg();
        superScore.request.image0 = *pimage0;
        superScore.request.image1 = *pimage1;

        cout << "rqeuest server " <<  count++ << endl;


        bool flag = client.call(superScore);

        vector<cv::Mat> images = {img0, img1};
        cv::Mat ret_img = ImageSplicing(images, 1);
        cv::imshow("image in server", ret_img);
        cv::waitKey(1);
//        r.sleep();

        if(flag){
            ROS_INFO_STREAM("请求正常处理,响应结果: " << superScore.response.score);
        }
        else
        {
            ROS_ERROR("请求处理失败....");
            return 1;
        }

        if(count>100){
            ros::shutdown();
        }

    }

    return 0;
}

